import math
import sys
import time
from collections import Counter
import numpy as np

# 定义地图
map_be_search = np.array(
    [
        # 0  1  2  3  4  5  6  7  8  9  10
        [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0],
        [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 1],
        [1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1],
        [1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1],
        [1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
        [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
    ]
)

# print(map_be_search)
map_be_search[1][6] = 3  # 起点
map_be_search[25][8] = 3  # 终点
# print(map_be_search)

# 边界
border_x_min = 0
border_x_max = 27
border_y_min = 0
border_y_max = 18


class BStarSearch:
    """
        B*寻路算法
    """

    def __init__(self):
        # 起点和终点
        self.start = {"point": (1, 6), "F": 0, "direct": None, "marker": 1}  # marker 代表特殊点
        self.end = {"point": (25, 8), "marker": 1}

        # # 开启的表     这里也不需要了，我们不管路径的话
        # self.openSet = []
        # # 关闭的表
        # self.closeSet = []

        # 边界
        self.border_x_min = 0
        self.border_x_max = 18
        self.border_y_min = 0
        self.border_y_max = 18

        self.special_point = self.start["point"]
        self.special_point_list = [self.start, ]

    #  计算点 椭圆上点  到 a(待变化)  b终点（这个点不需要传进来，初始化时我们指定了）          就行了，好像不需要穿透点....文章里虽然写了
    def get_distance_from_three_point(self, cur, a="", b=""):
        a = math.sqrt((cur[0] - self.end["point"][0]) ** 2 + (cur[1] - self.end["point"][1]) ** 2)
        b = math.sqrt((cur[0] - self.special_point[0]) ** 2 + (cur[1] - self.special_point[1]) ** 2)
        return round(a + b, 2)

    # 获取周围八个点的坐标
    def get_neighbors(self, x, y):
        up = x, y + 1
        down = x, y - 1
        left = x - 1, y
        right = x + 1, y

        left_up = x - 1, y + 1
        right_up = x + 1, y + 1
        left_down = x - 1, y - 1
        right_down = x + 1, y - 1
        result = [up, down, left, right, left_up, right_up, left_down, right_down]
        return [p for p in result if border_x_min < p[0] < border_x_max and border_y_min < p[1] < border_y_max]

    # 当前点指向终点的向量。  四个方向  通过斜率相近 得到方向向量（1,0），（-1,0）（0，-1）（0,1）,==四方向的不要，实际是有问题的，所以用下面8方向的==
    def ____get_direct(self, cur):
        x_sub, y_sub = (self.end["point"][0] - cur[0]), (self.end["point"][1] - cur[1])
        # 说明 垂直 x轴上， k = y_sub / x_sub  0为被除数
        if x_sub == 0:
            # 可能是 除以绝对值
            return x_sub, y_sub / abs(y_sub)
        # 计算斜率
        k = y_sub / x_sub
        if -1 / 2 <= k <= 1 / 2:
            if x_sub < 0:
                return (-1, 0)
            else:
                return (1, 0)
        else:
            if y_sub > 0:
                return (0, 1)
            else:
                return (0, -1)

    # 当前点指向终点的向量。 八方向  通过斜率相近得到方向向量（1,0）（-1,0）--（0，-1）（0,1）----(1,1)(-1,-1)---(-1, 1)(1, -1)
    def get_direct(self, cur):
        x_sub, y_sub = (self.end["point"][0] - cur[0]), (self.end["point"][1] - cur[1])
        # 说明 垂直 x轴上， k = y_sub / x_sub  0为被除数
        if x_sub == 0:
            # 除以绝对值
            return x_sub, y_sub / abs(y_sub)
        # 计算斜率
        k = y_sub / x_sub
        if 3 / 2 < k or k <= -3 / 2:
            if x_sub < 0:
                return (0, -1)
            else:
                return (0, 1)

        if 1 / 2 < k <= 3 / 2:
            if x_sub < 0:
                return (-1, -1)
            else:
                return (1, 1)

        if -1 / 2 < k <= 1 / 2:
            if x_sub < 0:
                return (-1, 0)
            else:
                return (1, 0)

        if -3 / 2 < k <= -1 / 2:
            if x_sub < 0:
                return (-1, 1)
            else:
                return (1, -1)

    # 爬墙路径    这里对于我们来说，只需要返回  最远点（maker 标记一下）和穿透点      两个点就可以了
    def obstacle_path(self, cur, obstacle_point: tuple):
        # 穿透点信息
        temp_point = cur["point"]
        direct = self.get_direct(temp_point)
        while True:
            # 传进来的点,沿着终点方向，穿透障碍，得到可以探索的第一个点     ：地图内的任意两点连线都不可能穿过地图边界
            end_point = temp_point[0] + direct[0], temp_point[1] + direct[1]
            if map_be_search[end_point[0]][end_point[1]] == 0:
                break
            temp_point = end_point
        end_info = {}
        end_info["point"] = end_point

        # -----------------------这里我们遍历内圈，   外圈也是一样的----------------------------------
        # 攀爬的伪穿透点信息    也就是穿透点的前一个障碍点
        obstacle_end_point = temp_point

        # 开启的表,
        openSet = [{"point": obstacle_point}, {"point": obstacle_point}]
        # 关闭的表
        closeSet = []

        # 因为两条路都要走，openSet有两个相同的点 所以在第二次取到时，计算下前面一条完整路径的长度
        path1_length = 0
        while openSet != []:
            # 切换到关闭列表
            cur = openSet.pop()
            cur["distance"] = self.get_distance_from_three_point(cur["point"])
            closeSet.append(cur)

            # # 当前点已经是 穿透后的点了， 则返回起点
            # #！！！！这里可以直接把closeSet里的起点干掉，必有一条路到终点，如果closeSet里没有起点，那么一条路，如果有则两条路，如果连终点都不在closeSet则无路  ！！！！！
            if cur["point"] == obstacle_end_point:
                cur = openSet.pop(0)

            # 因为两条路都要走，openSet有两个相同的点 所以在第二次取到时，计算下前面一条 path1  完整路径的长度
            if cur["point"] == obstacle_point:
                path1_length = len(closeSet)

            # # 实时显示一下地图障碍内圈 的行走信息
            # for p in closeSet:
            #     map_be_search[p["point"][0]][p["point"][1]] = 8
            # print(map_be_search)
            # time.sleep(1)

            neighbors = self.get_neighbors(cur["point"][0], cur["point"][1])
            next_point_info = {}
            # 对当前格相邻的8格中的每一个
            for neighbor in neighbors:
                # 第一次到达伪终点后，终点信息已经加到 closeSet 里面了，导致第二次不能加入，这里手动加入，并中断遍历
                if neighbor == obstacle_end_point:
                    closeSet.append({"point": neighbor, "distance": self.get_distance_from_three_point(neighbor)})
                    break

                # ==1 且不在关闭表内，，边界判定在获取邻居时做了
                if map_be_search[neighbor[0]][neighbor[1]] == 1 and neighbor not in [p["point"] for p in closeSet]:
                    neighbors_list = self.get_neighbors(neighbor[0], neighbor[1])
                    for neighbor_neighbor in neighbors_list:
                        # 如果该邻居周围的格子里有一个 0， 说明它在障碍边缘，
                        if map_be_search[neighbor_neighbor[0]][neighbor_neighbor[1]] == 0:
                            next_point_info["point"] = neighbor

                # 这里很巧妙。对第一个点，它周围是有两个或者三个点符合条件的分别属于两个分支     打断我们只取第一个， 第一个分支
                # 最开始的开启表内有两个同一起点，最开始取了一个，而且接下来每次从openset中取最后一个点，如过又取到了第一个点，说明回到原点，这下就取了第二个分支
                if next_point_info:
                    break
            if next_point_info:
                openSet.append(next_point_info)

        path_all_length = len(closeSet)
        # ----------获得了第一条路径的长度 和总长度，，我们可以切成两段，看看是不是有伪终点在里面，然后找出最小路径 最远距离点就可以了
        if path1_length == path_all_length:
            special_point1 = self.get_max_distance(closeSet[:path1_length])
            return end_info, special_point1

        index = [closeSet.index(p) for p in closeSet if p["point"] == obstacle_end_point]
        print(index, "==============")
        if index == []:
            return 0, 0
        else:
            if len(index) == 1 and index[0] <= path1_length:
                special_point1 = self.get_max_distance(closeSet[:path1_length])
                return end_info, special_point1

            if len(index) == 1 and index[0] > path1_length:
                special_point2 = self.get_max_distance(closeSet[path1_length:])
                return end_info, special_point2

            # ----通过两轮攀爬的 路径长度，， 舍去其中一个 end_point,留下一个即可
            if path1_length < path_all_length - path1_length:
                special_point1 = self.get_max_distance(closeSet[:path1_length])
                return end_info, special_point1
            else:
                special_point2 = self.get_max_distance(closeSet[path1_length:])
                return end_info, special_point2

    def get_max_distance(self, path, obstacle_inside_path=True):
        # a: [1, 3, 4, 5, 2, 7, 9]
        # 排序后[9, 7, 5, 4, 3, 2, 1]
        # 元素索引序列： [6, 5, 3, 2, 1, 4, 0]
        sorted_id = sorted(range(len(path)), key=lambda k: path[k]['distance'], reverse=True)
        # print('元素索引序列：', sorted_id)
        index = sorted_id[0]

        # 内圈 需要计算最远距离点的周围的可走点  在返回可走点的最远距离点
        if obstacle_inside_path:
            tmp = []
            distance_list = []
            neighbors = self.get_neighbors(path[index]["point"][0], path[index]["point"][1])
            for neighbor in neighbors:
                if map_be_search[neighbor[0]][neighbor[1]] == 0:
                    tmp.append(neighbor)
                    d = self.get_distance_from_three_point(neighbor)
                    distance_list.append(d)

            index = distance_list.index(max(distance_list))
            return {"point": tmp[index], "distance": distance_list[index], "maker": 1}
        return path[index]

    def b_star_search(self):
        cur = self.start
        while True:
            direct = self.get_direct(cur["point"])
            # 当前点  + 指向终点的指向向量   相加得到下一个点的坐标
            next_point = cur["point"][0] + direct[0], cur["point"][1] + direct[1]

            next_point_info = {}
            if map_be_search[next_point[0]][next_point[1]] == 1:
                # 这个点是障碍就爬墙  没爬过返回  0，0  返回一个穿透点 和 特殊点最远距离点
                end_point, special_point = self.obstacle_path(cur, next_point)
                if end_point == 0:
                    return 0
                else:
                    # 把椭圆A点更新一下为最远距离点，，B点一直是终点
                    self.special_point = special_point["point"]
                    self.special_point_list.append(special_point)
                    next_point_info = end_point
            else:
                # 更新这个点的信息
                next_point_info["point"] = next_point
            # 下一个点交换为当前点
            cur = next_point_info

            # 到达终点
            if next_point == self.end["point"]:
                self.special_point_list.append(self.end)
                # 把产出的 特殊点显示一下
                for p in self.special_point_list:
                    map_be_search[p["point"][0]][p["point"][1]] = 9
                print(map_be_search)
                return 1


if __name__ == '__main__':
    tt = BStarSearch()
    path = tt.b_star_search()
